package org.nashua.tt151;

import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.SimpleRobot;
import org.nashua.tt151.Controller.DualAction;
import org.nashua.tt151.Controller.MSKinect;
import org.nashua.tt151.RobotMap.Servo.Position;

public class ServoArm extends SimpleRobot {
    /* Hardware */
    public static Servo servoArm = new Servo(RobotMap.Servo.SERVO_ARM);
    public static DualAction joy = new DualAction(1);  // 1st Joystick Detected
    public static MSKinect leftArm = new MSKinect(MSKinect.LEFT_ARM);

    /**
     * This method is called once each time the robot enters autonomous mode.
     */
    public void autonomous() {
        reset();
        leftArm.addButtonListener(MSKinect.Button.LEFT_LEG_FWD, new Controller.ButtonListener() {
            public void onPress(boolean opt) {
                // Lower the Servo Arm when A is pressed
                servoArm.set(Position.Arm.DOWN);
            }
            public void onRelease(boolean opt) {
                // Raise the Servo Arm when A is released
                servoArm.set(Position.Arm.UP);
            }
        });
        while (isEnabled() && isAutonomous()) {
            leftArm.queryButtons();  // Required at the beginning of each loop
        }
        reset();
    }

    /**
     * This method is called once each time the robot enters operator control.
     */
    public void operatorControl() {
        reset();
        joy.addButtonListener(DualAction.Button.A, new Controller.ButtonListener() {
            public void onPress(boolean opt) {
                // Lower the Servo Arm when A is pressed
                servoArm.set(Position.Arm.DOWN);
            }
            public void onRelease(boolean opt) {
                // Raise the Servo Arm when A is released
                servoArm.set(Position.Arm.UP);
            }
        });
        while (isEnabled() && isOperatorControl()) {
            joy.queryButtons();  // Required at the beginning of each loop
        }
        reset();
    }

    public void reset() {
        joy.clearAll();  // Reset DualAction to verify that it has no control
        leftArm.clearAll();  // Reset MSKinect to verify that it has no control
        servoArm.set(Position.Arm.DEFAULT);  // Reset the Servo to default
    }
}